#include "depth_camera.hpp"

using namespace AgvObstaAvoid;
using namespace tinyxml2;

namespace AgvObstaAvoid {

DepthCamera::DepthCamera(rclcpp::Node::SharedPtr node):node_(node)
{}

bool DepthCamera::CameraReadXml(XMLElement *element)
{
    uEquipment camera_equip;
    RCLCPP_INFO_STREAM(rclcpp::get_logger("DepthCamera"), "init camera equipment xml param");
    try
    {
        camera_equip.name = element->Attribute("name") ? std::stoi(std::string(element->Attribute("name"))) : EVENT_INT;
        camera_equip.topicname = element->Attribute("topicname") ? element->Attribute("topicname") : EVENT_STRING;
        camera_equip.ex_params.x = element->Attribute("sensor_x") ? atof(element->Attribute("sensor_x")) : EVENT_INT;
        camera_equip.ex_params.y = element->Attribute("sensor_y") ? atof(element->Attribute("sensor_y")) : EVENT_INT;
        camera_equip.ex_params.z = element->Attribute("sensor_z") ? atof(element->Attribute("sensor_z")) : EVENT_INT;
        camera_equip.ex_params.roll = element->Attribute("roll") ? atof(element->Attribute("roll")) : EVENT_INT;
        camera_equip.ex_params.pitch = element->Attribute("pitch") ? atof(element->Attribute("pitch")) : EVENT_INT;
        camera_equip.ex_params.yaw = element->Attribute("yaw") ? atof(element->Attribute("yaw")) : EVENT_INT;
        camera_equip.ex_params.crop_xmin = element->Attribute("crop_xmin") ? atof(element->Attribute("crop_xmin")) : EVENT_INT;
        camera_equip.ex_params.crop_ymin = element->Attribute("crop_ymin") ? atof(element->Attribute("crop_ymin")) : EVENT_INT;
        camera_equip.ex_params.crop_zmin = element->Attribute("crop_zmin") ? atof(element->Attribute("crop_zmin")) : EVENT_INT;
        camera_equip.ex_params.crop_xmax = element->Attribute("crop_xmax") ? atof(element->Attribute("crop_xmax")) : EVENT_INT;
        camera_equip.ex_params.crop_ymax = element->Attribute("crop_ymax") ? atof(element->Attribute("crop_ymax")) : EVENT_INT;
        camera_equip.ex_params.crop_zmax = element->Attribute("crop_zmax") ? atof(element->Attribute("crop_zmax")) : EVENT_INT;
        
        XMLElement *area_node = element->FirstChildElement();
        while (area_node)
        {
            uArea area_tem;
            area_tem.name = area_node->Attribute("name") ? std::stoi(std::string(area_node->Attribute("name"))) : EVENT_INT;

            XMLElement *region_node = area_node->FirstChildElement();
            while (region_node)
            {
                uRegion region_tem;
                region_tem.name = region_node->Attribute("name") ? region_node->Attribute("name") : EVENT_STRING;

                XMLElement *point = region_node->FirstChildElement();
                while (point)
                {
                    zPoint point_tem;
                    point_tem.x = point->Attribute("x") ? atof(point->Attribute("x")) : EVENT_INT;
                    point_tem.y = point->Attribute("y") ? atof(point->Attribute("y")) : EVENT_INT;
                    point_tem.zMin = point->Attribute("zMin") ? atof(point->Attribute("zMin")) : EVENT_INT;
                    point_tem.zMax = point->Attribute("zMax") ? atof(point->Attribute("zMax")) : EVENT_INT;
                    region_tem.poinlist.push_back(point_tem);
                    point = point->NextSiblingElement();
                }
                area_tem.regionlist.push_back(region_tem);
                region_node = region_node->NextSiblingElement();
            }
            camera_equip.arealist[area_tem.name] = area_tem;
            area_node = area_node->NextSiblingElement();
        }
    }
    catch (const std::exception &e)
    {
        std::cerr << e.what() << '\n';
        RCLCPP_ERROR_STREAM(rclcpp::get_logger("DepthCamera"), "xml lidar read is error");
        return false;
    }

    camera_equip_list.push_back(camera_equip);
    RCLCPP_INFO_STREAM(rclcpp::get_logger("DepthCamera"), "camera equipment init xml param successed");

    ShowEquipParam(&camera_equip);
    return true;
}

void DepthCamera::ShowEquipParam(uEquipment *equipment)
{
    RCLCPP_INFO_STREAM(rclcpp::get_logger("DepthCamera"), "show the camera equipment param");
    RCLCPP_INFO_STREAM(rclcpp::get_logger("DepthCamera"), 
        "equip name:" << equipment->name << " topicname:" << equipment->topicname);
    RCLCPP_INFO_STREAM(rclcpp::get_logger("DepthCamera"), 
        "equip roll:" << equipment->ex_params.roll << " pitch:" << equipment->ex_params.pitch << " yaw:" << equipment->ex_params.yaw);

    for (auto item = equipment->arealist.begin(); item != equipment->arealist.end(); item++)
    {
        RCLCPP_INFO_STREAM(rclcpp::get_logger("DepthCamera"), "area name:" << item->first);
        for (size_t j = 0; j < item->second.regionlist.size(); j++)
        {
            RCLCPP_INFO_STREAM(rclcpp::get_logger("DepthCamera"), "region name: " << item->second.regionlist[j].name);
            for (size_t k = 0; k < item->second.regionlist[j].poinlist.size(); k++)
            {
                RCLCPP_INFO_STREAM(rclcpp::get_logger("DepthCamera"), 
                    "x:" << item->second.regionlist[j].poinlist[k].x);
                RCLCPP_INFO_STREAM(rclcpp::get_logger("DepthCamera"), 
                    "y:" << item->second.regionlist[j].poinlist[k].y);
                RCLCPP_INFO_STREAM(rclcpp::get_logger("DepthCamera"), 
                    "zMin:" << item->second.regionlist[j].poinlist[k].zMin);
                RCLCPP_INFO_STREAM(rclcpp::get_logger("DepthCamera"), 
                    "zMax:" << item->second.regionlist[j].poinlist[k].zMax);
            }
        }
    }
}

void DepthCamera::init()
{
    int size = camera_equip_list.size();
    RCLCPP_INFO_STREAM(rclcpp::get_logger("DepthCamera"), "camera equip size:" << size);
    
    auto cloud_callback_1 = [this](const sensor_msgs::msg::PointCloud2::SharedPtr msg) {
        this->CloudCallback(msg, camera_equip_list[0].ex_params, camera_equip_list[0].name, 1);
    };
    
    auto cloud_callback_2 = [this](const sensor_msgs::msg::PointCloud2::SharedPtr msg) {
        this->CloudCallback(msg, camera_equip_list[1].ex_params, camera_equip_list[1].name, 2);
    };
    
    auto cloud_callback_3 = [this](const sensor_msgs::msg::PointCloud2::SharedPtr msg) {
        this->CloudCallback(msg, camera_equip_list[2].ex_params, camera_equip_list[2].name, 3);
    };

    switch (size)
    {
    case 0:
        break;

    case 1:
        cloud_sub_1 = node_->create_subscription<sensor_msgs::msg::PointCloud2>(
            camera_equip_list[0].topicname, 1, cloud_callback_1);
        obstacle_pub_1 = node_->create_publisher<sensor_msgs::msg::PointCloud2>("depth_obstacle_1", 1);
        break;
        
    case 2:
        cloud_sub_1 = node_->create_subscription<sensor_msgs::msg::PointCloud2>(
            camera_equip_list[0].topicname, 1, cloud_callback_1);
        obstacle_pub_1 = node_->create_publisher<sensor_msgs::msg::PointCloud2>("depth_obstacle_1", 1);
        
        cloud_sub_2 = node_->create_subscription<sensor_msgs::msg::PointCloud2>(
            camera_equip_list[1].topicname, 1, cloud_callback_2);
        obstacle_pub_2 = node_->create_publisher<sensor_msgs::msg::PointCloud2>("depth_obstacle_2", 1);
        break;
        
    case 3:
        cloud_sub_1 = node_->create_subscription<sensor_msgs::msg::PointCloud2>(
            camera_equip_list[0].topicname, 1, cloud_callback_1);
        obstacle_pub_1 = node_->create_publisher<sensor_msgs::msg::PointCloud2>("depth_obstacle_1", 1);
        
        cloud_sub_2 = node_->create_subscription<sensor_msgs::msg::PointCloud2>(
            camera_equip_list[1].topicname, 1, cloud_callback_2);
        obstacle_pub_2 = node_->create_publisher<sensor_msgs::msg::PointCloud2>("depth_obstacle_2", 1);

        cloud_sub_3 = node_->create_subscription<sensor_msgs::msg::PointCloud2>(
            camera_equip_list[2].topicname, 1, cloud_callback_3);
        obstacle_pub_3 = node_->create_publisher<sensor_msgs::msg::PointCloud2>("depth_obstacle_3", 1);
        break;

    default:
        break;
    }
}

void DepthCamera::CloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr cloud_msg, 
                               const ExtrinsicParameter &ex, const int& equip_name, int topic_name)
{
    std::lock_guard<std::mutex> sc_mutex(cloud_mutex);
    static pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    static pcl::PointCloud<pcl::PointXYZ>::Ptr cropped_cloud(new pcl::PointCloud<pcl::PointXYZ>);

    cloud->clear();
    cropped_cloud->clear();

    pcl::fromROSMsg(*cloud_msg, *cloud);
    if (cloud->empty()) {
        RCLCPP_WARN_STREAM(rclcpp::get_logger("DepthCamera"), "Cloud is empty for equip_name: " << equip_name);
        return;
    }

    // Crop in original camera coordinate system
    pcl::CropBox<pcl::PointXYZ> crop_filter;
    crop_filter.setInputCloud(cloud);
    crop_filter.setMin(Eigen::Vector4f(ex.crop_xmin, ex.crop_ymin, ex.crop_zmin, 1.0));
    crop_filter.setMax(Eigen::Vector4f(ex.crop_xmax, ex.crop_ymax, ex.crop_zmax, 1.0));
    crop_filter.filter(*cropped_cloud);

    std::string frame_id = cloud_msg->header.frame_id;
    sensor_msgs::msg::PointCloud2 output_cloud;
    pcl::toROSMsg(*cropped_cloud, output_cloud);
    output_cloud.header.frame_id = "depth_obstacle_frame";
    
    switch (topic_name)
    {
    case 1:
        obstacle_pub_1->publish(output_cloud);
        lidar_time_1 = node_->now();
        break;
    case 2:
        obstacle_pub_2->publish(output_cloud);
        lidar_time_2 = node_->now();
        break;
    case 3:
        obstacle_pub_3->publish(output_cloud);
        lidar_time_3 = node_->now();
        break;
    default:
        break;
    }

    std::vector<Point> point_vector;
    for (size_t i = 0; i < cropped_cloud->points.size(); ++i)
    {
        Point point;
        point.x = cropped_cloud->points[i].z;
        point.y = -cropped_cloud->points[i].x;
        point.z = -cropped_cloud->points[i].y;
        point_vector.push_back(point);
    }
    
    scan_point.erase(equip_name);
    scan_point[equip_name] = point_vector;
}

std::vector<agv_msgs::msg::AvoidObsta> DepthCamera::CloudCheck(std::vector<ScenesJudge::cEquip> &camera_equip)
{
    std::lock_guard<std::mutex> sc_mutex(cloud_mutex);
    std::vector<agv_msgs::msg::AvoidObsta> avoid_msg;
    
    for (size_t i = 0; i < camera_equip.size(); i++)
    {
        if (scan_point.find(camera_equip[i].name) != scan_point.end())
        {
            agv_msgs::msg::AvoidObsta equip_avoid_msg;
            bool isWarn = false;
            bool isSlow = false;
            bool isStop = false;
            auto item = scan_point.find(camera_equip[i].name)->second;

            std::vector<zPoint> point_warn = RegionSeg(camera_equip[i].name, camera_equip[i].area, "warn");
            std::vector<zPoint> point_slow = RegionSeg(camera_equip[i].name, camera_equip[i].area, "slow");
            std::vector<zPoint> point_stop = RegionSeg(camera_equip[i].name, camera_equip[i].area, "stop");

            bool warn_status_ = Isenable("warn", camera_equip[i]);
            bool slow_status_ = Isenable("slow", camera_equip[i]);
            bool stop_status_ = Isenable("stop", camera_equip[i]);

            PolygonCache warn_cache, slow_cache, stop_cache;
            PrecomputeBoundingBox(point_warn, warn_cache);
            PrecomputeBoundingBox(point_slow, slow_cache);
            PrecomputeBoundingBox(point_stop, stop_cache);

            for (size_t j = 0; j < item.size(); j++)
            {
                if (!isStop && stop_status_) {
                    isStop = FastInPolygon(item[j], stop_cache);
                }
                if (!isSlow && slow_status_) {
                    isSlow = FastInPolygon(item[j], slow_cache);
                }
                if (!isWarn && warn_status_) {
                    isWarn = FastInPolygon(item[j], warn_cache);
                }

                if (isStop && isSlow && isWarn) break;
            }

            equip_avoid_msg.equipment = camera_equip[i].name;
            equip_avoid_msg.area = camera_equip[i].area;
            equip_avoid_msg.stop = isStop ? 1 : 0;
            equip_avoid_msg.slow = isSlow ? 1 : 0;
            equip_avoid_msg.warn = isWarn ? 1 : 0;
            
            avoid_msg.push_back(equip_avoid_msg);
        }
    }
    return avoid_msg;
}

bool DepthCamera::Isenable(std::string status, ScenesJudge::cEquip &equip)
{
    if (status == "warn")
    {
        return equip.warnenable;
    }
    else if (status == "slow")
    {
        return equip.slowenable;
    }
    else if (status == "stop")
    {
        return equip.stopenable;
    }
    return true;
}

bool DepthCamera::InPolygon(const Point &p, const std::vector<zPoint> &poly)
{
    int polySides = poly.size();
    
    float z = p.z;
    float global_zMin = std::numeric_limits<double>::max();
    float global_zMax = std::numeric_limits<double>::lowest();
    for (const auto &vertex : poly) {
        global_zMin = std::min(global_zMin, vertex.zMin);
        global_zMax = std::max(global_zMax, vertex.zMax);
    }
    if (p.z < global_zMin || p.z > global_zMax) 
    {
        return false;
    }

    float x = p.x, y = p.y;
    bool is_inside = false;
    int j = polySides - 1;

    for (size_t i = 0; i < polySides; ++i) {
        const float xi = poly[i].x, yi = poly[i].y;
        const float xj = poly[j].x, yj = poly[j].y;

        if (yi == yj) {
            j = i;
            continue;
        }

        if (xi == xj) {
            if ((yi <= y && yj >= y) || (yj <= y && yi >= y)) {
                if (xi < x) is_inside = !is_inside;
            }
            j = i;
            continue;
        }

        const bool y_in_range = (yi < y && yj >= y) || (yj < y && yi >= y);
        if (y_in_range) {
            const float slope = (xj - xi) / (yj - yi);
            const float intersect_x = xi + (y - yi) * slope;
            if (intersect_x < x) is_inside = !is_inside;
        }
        j = i;
    }
    
    return is_inside;
}

std::vector<DepthCamera::zPoint> DepthCamera::RegionSeg(const int& equip_name, const int& area_name, const std::string& region_name)
{
    std::vector<zPoint> poly_tem;
    for (size_t i = 0; i < camera_equip_list.size(); i++)
    {
        if (camera_equip_list[i].name == equip_name)
        {
            if (camera_equip_list[i].arealist.find(area_name) != camera_equip_list[i].arealist.end())
            {
                uArea area = camera_equip_list[i].arealist.find(area_name)->second;
                for (size_t j = 0; j < area.regionlist.size(); j++)
                {
                    if (area.regionlist[j].name == region_name)
                    {
                        for (size_t k = 0; k < area.regionlist[j].poinlist.size(); k++)
                        {
                            DepthCamera::zPoint u_point;
                            u_point.x = area.regionlist[j].poinlist[k].x;
                            u_point.y = area.regionlist[j].poinlist[k].y;
                            u_point.zMin = area.regionlist[j].poinlist[k].zMin;
                            u_point.zMax = area.regionlist[j].poinlist[k].zMax;
                            poly_tem.push_back(u_point);
                        }
                        return poly_tem;
                    }
                }
            }
        }
    }

    RCLCPP_INFO_STREAM(rclcpp::get_logger("DepthCamera"), "not found equip" << equip_name << " in laser_equip_list");
    return poly_tem;
}

void DepthCamera::PrecomputeBoundingBox(std::vector<zPoint>& poly, PolygonCache& cache) 
{
    cache.min_x = cache.min_y = cache.min_z = std::numeric_limits<double>::max();
    cache.max_x = cache.max_y = cache.max_z = std::numeric_limits<double>::lowest();
    for (const auto& p : poly) {
        cache.min_x = std::min(cache.min_x, p.x);
        cache.max_x = std::max(cache.max_x, p.x);
        cache.min_y = std::min(cache.min_y, p.y);
        cache.max_y = std::max(cache.max_y, p.y);
        cache.min_z = std::min(cache.min_z, p.zMin);
        cache.max_z = std::max(cache.max_z, p.zMax);
    }
    cache.points = poly;
}

bool DepthCamera::FastInPolygon(const Point& p, const PolygonCache& cache) 
{
    if (p.z < cache.min_z || p.z > cache.max_z) return false;
    if (p.x < cache.min_x || p.x > cache.max_x) return false;
    if (p.y < cache.min_y || p.y > cache.max_y) return false;
    
    return InPolygon(p, cache.points);
}

bool DepthCamera::TimeoutCheck(const builtin_interfaces::msg::Time& time)
{
    int lidar_size = camera_equip_list.size();
    for (size_t i = 0; i < lidar_size; i++)
    {
        if (0 == i && (rclcpp::Time(time.sec, time.nanosec) - rclcpp::Time(lidar_time_1.sec, lidar_time_1.nanosec) > rclcpp::Duration(2, 0)))
        {
            RCLCPP_INFO_STREAM(rclcpp::get_logger("DepthCamera"), 
                "depth_camera equipment:" << i << " data time out");
            return true;
        }

        if (1 == i && (rclcpp::Time(time.sec, time.nanosec) - rclcpp::Time(lidar_time_2.sec, lidar_time_2.nanosec) > rclcpp::Duration(2, 0)))
        {
            RCLCPP_INFO_STREAM(rclcpp::get_logger("DepthCamera"), 
                "depth_camera equipment:" << i << " data time out");
            return true;
        }

        if (2 == i && (rclcpp::Time(time.sec, time.nanosec) - rclcpp::Time(lidar_time_3.sec, lidar_time_3.nanosec) > rclcpp::Duration(2, 0)))
        {
            RCLCPP_INFO_STREAM(rclcpp::get_logger("DepthCamera"), 
                "depth_camera equipment:" << i << " data time out");
            return true;
        }
    }
    
    return false;
}

bool DepthCamera::ObbIntersect(const OBB& a, const OBB& b) {
    Eigen::Vector3f delta = b.center - a.center;

    Eigen::Vector3f axes[15] = {
        a.axes[0], a.axes[1], a.axes[2],
        b.axes[0], b.axes[1], b.axes[2],
        a.axes[0].cross(b.axes[0]),
        a.axes[0].cross(b.axes[1]),
        a.axes[0].cross(b.axes[2]),
        a.axes[1].cross(b.axes[0]),
        a.axes[1].cross(b.axes[1]),
        a.axes[1].cross(b.axes[2]),
        a.axes[2].cross(b.axes[0]),
        a.axes[2].cross(b.axes[1]),
        a.axes[2].cross(b.axes[2])
    };

    for (const auto& axis : axes) {
        if (axis.squaredNorm() < 1e-6) continue;
        Eigen::Vector3f norm_axis = axis.normalized();

        float ra = a.extents[0] * std::abs(a.axes[0].dot(norm_axis)) +
                   a.extents[1] * std::abs(a.axes[1].dot(norm_axis)) +
                   a.extents[2] * std::abs(a.axes[2].dot(norm_axis));

        float rb = b.extents[0] * std::abs(b.axes[0].dot(norm_axis)) +
                   b.extents[1] * std::abs(b.axes[1].dot(norm_axis)) +
                   b.extents[2] * std::abs(b.axes[2].dot(norm_axis));

        float distance = std::abs(delta.dot(norm_axis));

        if (distance > ra + rb) return false;
    }

    return true;
}

void DepthCamera::ComputeOBB(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, OBB& obb) {
    if (cloud->empty()) {
        std::cerr << "cloud is empty" << std::endl;
        return;
    }

    pcl::PCA<pcl::PointXYZ> pca;
    pca.setInputCloud(cloud);
    Eigen::Matrix3f eigenvectors = pca.getEigenVectors();
    Eigen::Vector3f eigenvalues = pca.getEigenValues();

    obb.axes[0] = eigenvectors.col(0).normalized();
    obb.axes[1] = eigenvectors.col(1).normalized();
    obb.axes[2] = eigenvectors.col(2).normalized();

    Eigen::Vector4f centroid;
    pcl::compute3DCentroid(*cloud, centroid);
    obb.center = centroid.head<3>();

    Eigen::Vector3f min_proj, max_proj;
    min_proj.setConstant(std::numeric_limits<float>::max());
    max_proj.setConstant(std::numeric_limits<float>::lowest());

    for (const auto& point : *cloud) {
        Eigen::Vector3f p = point.getVector3fMap() - obb.center;
        for (int i = 0; i < 3; ++i) {
            float projection = p.dot(obb.axes[i]);
            if (projection < min_proj[i]) min_proj[i] = projection;
            if (projection > max_proj[i]) max_proj[i] = projection;
        }
    }

    obb.extents[0] = (max_proj[0] - min_proj[0]) / 2.0f;
    obb.extents[1] = (max_proj[1] - min_proj[1]) / 2.0f;
    obb.extents[2] = (max_proj[2] - min_proj[2]) / 2.0f;
}

DepthCamera::AABB DepthCamera::ComputeAABB(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud) 
{
    AABB box;
    pcl::PointXYZ min_pt, max_pt;
    pcl::getMinMax3D(*cloud, min_pt, max_pt);
    box.min_x = min_pt.x;
    box.min_y = min_pt.y;
    box.min_z = min_pt.z;
    box.max_x = max_pt.x;
    box.max_y = max_pt.y;
    box.max_z = max_pt.z;
    return box;
}

bool DepthCamera::AabbCollision(const AABB& box1, const AABB& box2)
{
    bool x_overlap = (box1.max_x > box2.min_x) && (box1.min_x < box2.max_x);
    bool y_overlap = (box1.max_y > box2.min_y) && (box1.min_y < box2.max_y);
    bool z_overlap = (box1.max_z > box2.min_z) && (box1.min_z < box2.max_z);
    return x_overlap && y_overlap && z_overlap;
}
}// end namespace